#ifndef BIPEDROBIN_LOCAL_PLANNER_BIPEDROBIN_PLANNER_ROS_H_
#define BIPEDROBIN_LOCAL_PLANNER_BIPEDROBIN_PLANNER_ROS_H_
#include <boost/shared_ptr.hpp>
#include <nav_core/base_local_planner.h>
#include <std_msgs/UInt8.h>
#include <bipedRobin_msgs/StepTarget3DService.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <footstep_planner/FootstepPlanner.h>
#include <footstep_planner/State.h>

namespace bipedRobin_local_planner {
  /**
   * @class BipedRobinPlannerROS
   * @brief ROS Wrapper for the BipedRobinPlanner that adheres to the BaseLocalPlanner interface and can be used as a plugin for move_base.
   */
  class BipedRobinPlannerROS : public nav_core::BaseLocalPlanner {
		public:
		/**
		* @brief  Constructor for BipedRobinPlannerROS wrapper
		*/
		BipedRobinPlannerROS() : costmap_ros_(NULL), tf_(NULL), initialized_(false) {}

		/**
		* @brief  Constructs the ros wrapper
		* @param name The name to give this instance of the trajectory planner
		* @param tf A pointer to a transform listener
		* @param costmap The cost map to use for assigning costs to trajectories
		*/
		void initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros);

		/**
		* @brief  Check if the goal pose has been achieved
		* @return True if achieved, false otherwise
		*/
		bool isGoalReached();

		/**
		* @brief  Set the plan that the controller is following
		* @param orig_global_plan The plan to pass to the controller
		* @return True if the plan was updated successfully, false otherwise
		*/
		bool setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan);

		/**
		* @brief  Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base
		* @param cmd_vel Will be filled with the velocity command to be passed to the robot base
		* @return True if a valid trajectory was found, false otherwise
		*/
		bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel);
		
		void stepsLeftCallback(const std_msgs::UInt8 stepsLeftInBuffer);

	private:
		inline double sign(double x){
		return x < 0.0 ? -1.0 : 1.0;
		}
		
		
		void planningThread(void);
		void updateMapToPlanner();
		bool initializeNavigator();
		void  getFootstepFromState(const footstep_planner::State& from,  const footstep_planner::State& to, bipedRobin_msgs::StepTarget3D* footstep);

		/**
		* @brief  Callback for receiving odometry data
		* @param msg An Odometry message 
		*/
		//void odomCallback(const nav_msgs::Odometry::ConstPtr& msg);

		/**
		* @brief Callback to set a simulated robot at a certain pose.
		*
		* Subscribed to 'goal'.
		*/
		void goalPoseCallback(const geometry_msgs::PoseStampedConstPtr& goal_pose);

		/**
		* @brief Callback to set the map.
		*
		* Subscribed to 'map'.
		*/
		void mapCallback(const nav_msgs::OccupancyGridConstPtr& occupancy_map);

		costmap_2d::Costmap2DROS* costmap_ros_;
		tf::TransformListener* tf_;
		double max_vel_th_, min_vel_th_, min_rot_vel_;
		double rot_stopped_vel_, trans_stopped_vel_;
		double yaw_goal_tolerance_, xy_goal_tolerance_;
		bool prune_plan_;
		bool initialized_;
		int ivSendStep;
		bool ivPlanSteps;
		
		ros::Publisher g_plan_pub_, l_plan_pub_; //for local and global plan to publish
		ros::Publisher ivMapPub; //for local occupancy grid
		ros::ServiceClient ivFootstepIncSrv;
		ros::Subscriber stepsLeftInBufferSub;
		
		footstep_planner::FootstepPlanner ivPlanner;

		ros::ServiceServer ivFootstepPlanService;
		boost::thread* ivFootstepPlanningThread;
		boost::mutex ivFootstepPlanningMutex;
		boost::mutex planner_mutex_;
		boost::mutex localPlan_mutex_;

		std::string ivIdFootRight;
		std::string ivIdFootLeft;
		std::string ivIdMapFrame;
		double ivPlanningDistance;

		std::list <footstep_planner::State> ivStepsToStart;
		footstep_planner::State ivLeftLegStart;
		footstep_planner::State ivRightLegStart;
		footstep_planner::State ivLeftLegCurrent;
		footstep_planner::State ivRightLegCurrent;
		double ivLocalGoalX;
		double ivLocalGoalY;
		double ivLocalGoalTheta;

		std::vector<geometry_msgs::PoseStamped> global_plan_;
	};
};
#endif
